#include "px4_flight_handler.h"


#define SBUF_SIZE 					512
#define GCS_MAVLINK_CHAN			1



using namespace std;


int64_t getTimeMillis()
{
	int64_t timeMs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

	return timeMs;
}


void sleepMs(uint32_t timeMs)
{
	usleep(timeMs * 1000);
}

double wrap360(const double angle)
{
    double res = fmod(angle, 360.0);
    if (res < 0)
    {
        res += 360.0;
    }
    return res;
}



double wrap180(const double angle)
{
    auto res = wrap360(angle);
    if (res > 180)
	{
        res -= 360;
    }
    return res;
}


px4FlightHandler::px4FlightHandler():
	m_connect(false),
	m_udpConnectFlag(false),
	m_uartConnectFlag(false),
	m_logFp(NULL),
	m_getLogDirFlag(false),
	m_lastLogNum(0)
{

}


px4FlightHandler::~px4FlightHandler()
{
	if(NULL != m_logFp)
	{
		fflush(m_logFp);
		fclose(m_logFp);
		m_logFp = NULL;
	}
}



bool px4FlightHandler::startUpSystem(const char *connectType)
{
	bool ret;

	if(!strcmp(connectType, "uart"))
	{
		m_uartConnectFlag = true;
	}
	else if(!strcmp(connectType, "udp"))
	{
		m_udpConnectFlag = true;
	}
	else
	{
		cout << "connectType is error:" << connectType << endl;
		return false;
	}

	// start px4 control module log
	ret = getLogDir();
	if(!ret)
	{
		cout << "getLogDir failed" << endl;
	}
	else
	{
		ret = createLogFile();
		if(!ret)
		{
			cout << "createLogFile failed" << endl;
		}
	}


	if(m_uartConnectFlag)
	{
		//start uart
		ret = startUartConnect();
	}
	else
	{
		//start udp
		ret = startUdpConnect();
	}

	return ret;
}


bool px4FlightHandler::startUdpConnect()
{
	bool ret;

	ret = udpClient("127.0.0.1", "14550");

	if(!ret)
	{
		return false;
	}

	//connect test
	uint8_t buf[512];
	int count=30; // wait  30 seconds

	mavlink_message_t msg;
	do{
		sleepMs(1000);

		bzero(buf, sizeof(buf));
		socklen_t addr_len = sizeof(struct sockaddr_in);
		int16_t nbytes = recvfrom(m_udpSockfd, buf, sizeof(buf), 0, (struct sockaddr *)&m_saddr, &addr_len);

		if(nbytes < 0)
		{
			printf("udp recv error \n");
			close(m_udpSockfd);
			m_udpSockfd = -1;
			break;
		}

		if(nbytes > 0)
		{
			for(int i=0; i<nbytes; i++)
			{
				if(mavlink_parse_char(GCS_MAVLINK_CHAN, buf[i], &msg, &m_mavlinkStatus))
				{
					cout << "msgid:" << msg.msgid << endl;
					switch(msg.msgid)
					{
						case MAVLINK_MSG_ID_HEARTBEAT:
						{
							m_systemId = msg.sysid;
							m_compid = msg.compid;
							printf("sysid:%d, compid:%d\n", m_systemId, m_compid);
							writeLogFile("sysid:%d, compid:%d\n", m_systemId, m_compid);
							m_connect = true;
							//Subscribe to mavlink messages
							subcribeMavlinkMessages();
							break;
						}
						default:
							break;
					}
				}
			}
		}

	}while(count-- && !m_connect);

	if(false == m_connect)
	{
		writeLogFile("udp test mavlink message failed\n");
		return false;
	}

	return true;
}

bool px4FlightHandler::startUartConnect()
{

}



bool px4FlightHandler::udpClient(const char *ip, const char *port)
{
	m_udpSockfd=socket(AF_INET,SOCK_DGRAM,0);
	if(m_udpSockfd < 0)
	{
		writeLogFile("udp socket error\n");
		return false;
	}

	memset(&m_saddr,0,sizeof(m_saddr));
	m_saddr.sin_family = AF_INET;
	m_saddr.sin_port = htons(atoi(port));
	m_saddr.sin_addr.s_addr = inet_addr(ip);

	int ret =bind(m_udpSockfd, reinterpret_cast<sockaddr*>(&m_saddr), sizeof(m_saddr));
	if(ret < 0)
	{
		close(m_udpSockfd);
		m_udpSockfd = -1;
		printf("tcp connect error\n");
		return false;
	}

	return true;
}

void px4FlightHandler::subcribeMavlinkMessages()
{

}


bool px4FlightHandler::getLogDir()
{
	if (0 == access("./px4_control_module_log", F_OK))
	{
		m_getLogDirFlag = true;
		return true;
	}
	else
	{
		int ret = mkdir("./px4_control_module_log", 0777);

		if(0 == ret)
		{
			m_getLogDirFlag = true;
			return true;
		}
		else
		{
			m_getLogDirFlag = false;
			return false;
		}
	}

	return false;
}


bool px4FlightHandler::createLogFile()
{
	if(!m_getLogDirFlag)
	{
		return false;
	}

	for(int i=1; i<1000; i++)
	{
		char logName[50];
		memset(logName, 0, sizeof(logName) / sizeof(logName[0]));
		sprintf(logName,"./px4_control_module_log/px4_control_logs_%d.txt", i);
		m_lastLogNum = i;
		if(0 == access(logName, F_OK))
		{
			continue;
		}
		else
		{
			if(999 == i)
			{
				if(0 == system("rm  ./px4_control_module_log/*  -rf"))
				{
					m_logFp =  fopen("./px4_control_module_log/px4_control_logs_.txt", "ab+");
					if(NULL == m_logFp)
					{
						m_lastLogNum = 0;
						cout << "createlogFile failed1" << endl;
						return false;
					}
					m_lastLogNum = 1;
					return true;
				}
				else
				{
					m_lastLogNum--;
					cout << "rm ./px4_control_module_log/* -rf failed" << endl;
					m_logFp = NULL;
					return false;
				}
			}
			else
			{
				m_logFp =  fopen(logName, "ab+");
				if(NULL == m_logFp)
				{
					m_lastLogNum--;
					cout << "createLogFile failed2" << endl;
					return false;
				}
				return true;
			}
		}
	}

	return false;
}


bool px4FlightHandler::writeLogFile(const char * format, ...)
{
	size_t size, n;
	char sbuf[SBUF_SIZE];

	if(NULL == m_logFp)
	{
		return false;
	}

	va_list args;
	va_start (args, format);
	n = vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	size = fwrite(sbuf, 1, n, m_logFp);
    if (size != n) {
        fclose(m_logFp);
		m_logFp = NULL;
		cout << "writeLogFile fialed" << endl;
        return false;
    }

	return true;
}


void px4FlightHandler::recodeCpuTemperature()
{
	FILE *file = fopen("/sys/class/thermal/thermal_zone0/temp", "r");
	if(NULL != file)
	{
		char buf[10];
		float temp;
		memset(buf, 0, 10);
		size_t len = fread(buf, 1, 10, file);
		if(len > 0)
		{
			temp  = atof(buf);
			writeLogFile("cpu temperature is %f\n", temp / 1000.0f);
		}
		fclose(file);
	}
}


void px4FlightHandler::fflushLogFile()
{
	if(NULL != m_logFp)
	{
		fflush(m_logFp);
	}
}


px4FlightHandler flightHandler;
